package simplescenegraph.math;

/**
 *
 * SRT-Transform
 * 
 * @author Pedro Boechat
 *
 */
public class Transform {

	private Matrix3f s = new Matrix3f();
	
	private Matrix3f r = new Matrix3f();
	
	private Vector3f t = new Vector3f();
	
	public Transform() {
		s.setIdentity();
		r.setIdentity();
	}
	
	public Transform(Transform t0) {
		s = new Matrix3f(t0.s);
		r = new Matrix3f(t0.r);
		t = new Vector3f(t0.t);
	}
	
	public void scaleX(float f0) {
		s.multiply(new Matrix3f(f0, 0, 0, 0, 1, 0, 0, 0 ,1));
	}
	
	public void scaleY(float f0) {
		s.multiply(new Matrix3f(1, 0, 0, 0, f0, 0, 0, 0 ,1));
	}
	
	public void scaleZ(float f0) {
		s.multiply(new Matrix3f(1, 0, 0, 0, 1, 0, 0, 0 ,f0));
	}
	
	public void scale(float f0, float f1, float f2) {
		s.multiply(new Matrix3f(f0, 0, 0, 0, f1, 0, 0, 0 ,f2));
	}
	
	public void rotateX(float a0) {
		double r0 = Math.toRadians(a0);
		float c = (float) Math.cos(r0);
		float s = (float) Math.sin(r0);		
		r.multiply(new Matrix3f(1, 0, 0, 0, c, -s, 0, s, c));
	}
	
	public void rotateY(float a0) {
		double r0 = Math.toRadians(a0);
		float c = (float) Math.cos(r0);
		float s = (float) Math.sin(r0);		
		r.multiply(new Matrix3f(c, 0, s, 0, 1, 0, -s, 0, c));
	}
	
	public void rotateZ(float a0) {
		double r0 = Math.toRadians(a0);
		float c = (float) Math.cos(r0);
		float s = (float) Math.sin(r0);
		r.multiply(new Matrix3f(c, -s, 0, s, c, 0, 0, 0, 1));
	}
	
	public void rotateAxis(float a0, Vector3f a1) {
		/*
		 * 			| cosA + (1 - cosA) * a1^2 			(1 - c) * a1 * a2 - s * a3			(1 - c) * a1 * a3 + sinA * a2 
		 * Ra(A) =  |(1 - c) * a1 * a2 + sinA * a3 		cosA + (1 - cosA) * a2^2			(1 - c) * a1 * a2 - sinA * a3
		 * 			|(1 - c) * a1 * a3 - sinA * a2		(1 - c) * a2 * a3 + sinA * a1		cosA + (1 - cosA) * a3^2
		 */
		double r0 = Math.toRadians(a0);
		float c = (float) Math.cos(r0);
		float s = (float) Math.sin(r0);
		float p1 = (1 - c) * (a1.x * a1.y),
			  p2 = s * a1.z,
			  p3 = (1 - c) * (a1.x * a1.z),
			  p4 = s * a1.y,
			  p5 = (1 - c) * (a1.y * a1.z),
			  p6 = s * a1.x;
		r.multiply(new Matrix3f(c + (1 - c) * (a1.x * a1.x), 	p1 - p2, 						p3 + p4,
								p1 + p2, 						c + (1 - c) * (a1.y * a1.y),	p5 - p6,								
								p3 - p4,						p5 + p6,						c + (1 - c) * (a1.z * a1.z)));
	}
	
	public void reset() {
		s.setIdentity();
		r.setIdentity();
		t = new Vector3f();
	}
	
	public Vector3f getTranslate() {
		return t;
	}
	
	public void setTranslate(Vector3f t0) {
		t = new Vector3f(t0);
	}
	
	public Matrix3f getRotate() {
		return r;
	}
	
	public void setRotate(Matrix3f r0) {
		this.r = r0;
	}
	
	public Matrix3f getScale() {
		return s;
	}
	
	public void setScale(Matrix3f s0) {
		s = s0;
	}
	
	public float getHighestScaleFactor() {
		float f0 = s.m[0];
		if (f0 < s.m[4])
			f0 = s.m[4];
		if (f0 < s.m[8])
			f0 = s.m[8];
		
		return f0;
	}
	
	public void apply(Transform t0) {
		s.multiplyOther(t0.s);
		r.multiplyOther(t0.r);
		transform(t0.t);
	}
	
	public void transform(Vector3f v0) {
		v0.multiply(s);
		v0.multiply(r);
		v0.add(t);
	}
	
	public void transform(Matrix3f m0) {
		m0.multiply(s);
		m0.multiply(r);
		
		//TODO: Improve this!
		Vector3f v0 = m0.getColumn(0);
		v0.add(t);
		m0.setColumn(0, v0);
		v0 = m0.getColumn(1);
		v0.add(t);
		m0.setColumn(1, v0);
		v0 = m0.getColumn(2);
		v0.add(t);
		m0.setColumn(2, v0);
	}
	
	public void translate(float f0, float f1, float f2) {
		t.add(new Vector3f(f0, f1, f2));
	}
	
	public boolean equals(Object obj0) {
		Transform t0 = (Transform) obj0;
		return s.equals(t0.s) && r.equals(t0.r) && t.equals(t0.t);
	}
	
	public String toString() {
		return s.toString() + '\n' + r.toString() + '\n' + t.toString();
	}
	
}
